.TITLE DRIVE

;DEFINED GLOBALS

.GLOBL DRIVE,CLKSER,PTIME,ANGLES,DACOUT,RELBRK,SETBRK

;REQUIRED GLOBAL DEFINITIONS

.GLOBL DR11S,DR11O,DR11I,STTMDE,ENABLE,BRKMDE,ARMS,CANPRO
.GLOBL BSCALE,BSHIFT,BOFFST,ADCMDE,ADCENB,JANGLE
.GLOBL ISON,NOHDWR,SCALE,OFFSET,UNLOCK,LOCK
.GLOBL WORKNG,ADCDNE,ADCERR,STPARM,STRWRK,CHKDNE,NOINTG
.GLOBL NOTIME,RUNERR,FINAL,MODES,TICKLE,TICLER
.GLOBL RUN,TYPERR,PANIC,PANBUT

;REGISTER DEFINTIONS

R0	=%0
R1	=%1
R2	=%2
R3	=%3
R4	=%4
R5	=%5
SP	=%6
PC	=%7
;"DRIVE"  - SUBR. TO GENERATE AND CONTROL JOINT MOTIONS

;THIS ROUTINE RELEASES THE JOINT BRAKES AND OUTPUTS THE DAC VALUES
;FOR A GIVEN JOINT MOTION.  "DRIVE" REQUIRES AS ITS ONLY ARGUMENT A
;POINTER TO A TABLE OF MOTION VARIABLES.  A DESCRIPTION OF THE MOTION
;VARIABLE TABLE FOLLOWS:
;
;	DRVLST→	JTMASK	;BIT MASK TO CHECK IF ALL REQUIRED JOINTS ARE
;			;  WITHIN FINAL ERROR TOLERANCE.  THIS TEST
;			;  IS USED TO TERMINATE A NORMAL MOTION.
;		TTIME	;TOTAL PERMITTED JOINT MOTION TIME IN 10
;			;  MILLISECOND COUNTS.  A TIME OUT ERROR IS
;			;  AFTER TTIME*10 MILLISECONDS
;		RUNSUB	;ADDR. OF SUBR TO BE CALLED EVERY 20 MSEC TO
;			;  UPDATE THE JOINT SET POINTS. ALL REGISTERS
;			;  ARE AVAILABLE FOR USE, R0 ← SET PT. TIME,
;			;  R1 ← PTR TO JOINT ANGLE BUFFER.
;		BRAKES	;BRAKE BITS TO BE RELEASED AND LATER SET
;		FSTJT	;LOGICAL NUMBER OF FIRST JOINT TO BE SERVOED
;		NUMJT	;TOTAL NUMBER OF JOINTS TO BE SERVOED
;
;ALL JOINTS STARTING WITH FSTJT AND ENDING WITH FSTJT+NUMJT-1 ARE
;SERVOED EACH 10 MSECS. UNTIL EITHER THE MOTION IS COMPLETED,THE
;PANIC BUTTON IS HIT, OR A TIME OUT ERROR OCCURS.  THE IN RANGE BITS
;FOR THE JOINTS THAT ARE BEING SERVICED ARE NOT CHECKED UNTIL AFTER
;THE "FINAL" BIT IS SET IN THE 8TH WORD OF ONE OF THE JT ANGLE
;ARRAYS THAT ARE PASSED TO THE "RUNSUB" ROUTINE.  THE "RUNSUB"
;ROUTINE SHOULD SET THE "FINAL" BIT AFTER ALL INCREMENTAL SET
;POINTS HAVE BEEN COMPUTED.  IN BETWEEN CALLS TO "RUNSUB", LINEAR
;INTERPOLATION IS USED TO GENERATE INTERMEDIATE SET POINTS. A
;SAMPLE CALLING SEQUENCE TO "DRIVE" FOLLOWS:
;
;		MOV	#DRVLST,R1
;		JSR	PC,DRIVE
;
;"DRIVE" INDICATES ANY ERROR CONDITIONS GENERATED DURING THE MOTION
;BY LEAVING BITS SET IN THE ARM STATUS WORD "ARMS".

;REGISTERS USED:
;	R1 PASSES ARGUMENTS AND IS ALTERED

;DEFINITIONS:

JTMASK	=0
TTIME	=2
RUNSUB	=4
BRAKES	=6
FSTJT	=10
NUMJT	=12

DRIVE:	MOV	R0,-(SP)
	MOV	R2,-(SP)	
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R1,R4		;SAVE PTR TO DRIVE DATA
	MOV	R1,@#DRVLST
	MOV	#-2,@#PTIME	;PRESENT TIME IN 10 MSEC COUNTS
       	MOV	FSTJT(R4),R0	;INIT. DACS TO THE CURRENT POSITION
	MOV	NUMJT(R4),R1
	JSR	PC,DACOUT
	MOV	BRAKES(R4),R0	;BRAKE BITS
	BIS	@#MODES,R0	;SET ANY SPECIAL MODES REQUIRED
	JSR	PC,RELBRK	;RELEASE Jt. BRAKES, ENABLE INTERFACE
	BCS	DRVDNE		;BRANCH IF COULDN'T ENABLE INTERFACE
	CLR	@#DNECNT	;# OF SEQUENTIAL TIMES IN RANGE
	MOV	#BUF1+2,R0	;INIT JT ANGLE BUFFERS
	MOV	#BUF2+2,R1
	MOV	#7,R3		;INITIALIZE BOTH TO CURRENT LOCATIONS
	MOV	#JANGLE,R2
BUFINT:	MOV	(R2),(R0)+
	MOV	(R2)+,(R1)+
	SOB	R3,BUFINT
	CLR	(R0)
	CLR	(R1)
	MOV	#RUN,@#ARMS	;INDICATE ARM IN MOTION
	MTPS	#UNLOCK		;LET THE CLOCK INTERRUPT
AWAIT:	JSR	PC,TICKLE	;HAS SOMEONE TYPED ON THE TTY?
	BCC	CHKRUN		;NO
	BIS	#CANPRO,@#ARMS	;YES, STOP ARM MOTION
	MOVB	#TICLER,@#ARMS
CHKRUN:	BIT 	#RUN,@#ARMS	;ARM MOTION STOPPED?
	BNE	AWAIT		;NO
	MTPS	#LOCK		;STOP THE CLOCK INTERRUPTS
DRVDNE:	CLR	@#MODES		;MODES ONLY GOOD ONE TIME
	MOVB	@#ARMS,R1	;ERROR MESSAGE INDEX
	BEQ	.+6
	JSR	PC,TYPERR	;TYPE OUT ANY NECESSARY ERROR MESS.
	MOV	(SP)+,R4
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	MOV	(SP)+,R0
	RTS	PC

;END OF "DRIVE"
;"CLKSER" - CLOCK INTERRUPT ROUTINE

CLKSER:	MOV	R0,-(SP)	;SAVE ALL REGISTERS
	MOV	R1,-(SP)
	MOV	R2,-(SP)
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)

	BIT	#RUN,@#ARMS	;ARM STILL IN MOTION?
	BNE	.+6
	JMP	CLKDNE		;BRANCH IF NOT
	
	MOV	#STTMDE,@#DR11S	;FETCH SUBDEVICE BITS
	MOV	@#DR11I,R0
	MOV	@#DRVLST,R4	;GUT POINTER TO DRIVE DATA

	TSTB	@#ARMS		;STOP ARM MOTION?
	BEQ	.+6
	JMP	STPJT 		;BRANCH IF SO
	
	BIT	#PANIC,R0	;PANIC BUTTON HIT?
	BNE	CHKENB		;BRANCH IF NOT
	MOV	#PANBUT,R1	;ELSE INDICATE ERROR AND STOP RUNNING
	MOV	#CANPRO,R0
	BR	SIGERR

CHKENB:	BIT	#ISON,R0	;HARDWARE SERVO STILL ENABLED?
	BNE	CHKTME		;BRANCH IF STILL ON
	MOV	#NOHDWR,R1	;ELSE INDICATE ERROR AND STOP RUNNING
	MOV	#CANPRO,R0
	BR	SIGERR

CHKTME:	INC	@#PTIME		;INCREMENT TIME COUNT
	CMP	@#PTIME,TTIME(R4)	;OUT OF TIME?
	BLE	JTIN1 		;BRANCH IF OK
	MOV	#NOTIME,R1	;ELSE TIME OUT ERROR
	MOV	#CANPRO,R0
	BR	SIGERR

JTIN1:	BIT	#CHKDNE,@#ARMS	;WAITING FOR JOINTS TO FINISH?
	BEQ	NEWSET		;BRANCH IF STILL MOVING SET POINTS
	INC	@#DNECNT	;ASSUME JOINTS WITHIN FINAL RANGE
	COM	R0		;IN RANGE?
	BIT	JTMASK(R4),R0
	BEQ	.+6		;SKIP IF IN RANGE
	CLR	@#DNECNT	;ELSE ZERO IN RANGE COUNT
	COM	R0
	CMP	#3,@#DNECNT	;DON'T STOP TILL IN RANGE 3 TIMES
	BLE	STPJT		;  IN A ROW
	BR	SETDAC		;NOT DONE YET, REFRESH THE DAC VALUES

NEWSET:	MOV	#JANGLE,R1	;UPDATE SET POINT
	MOV	#7,R2		;7 JOINTS IN ALL
	BIT	#1,@#PTIME	;ODD/EVEN SERVOING PERIOD?
	BEQ	CALRUN		;EVEN
	MOV	@CBUF,R0	;ODD, SWITCH TO NEXT JOINT ANGLES
	MOV	R0,@#CBUF
	TST	(R0)+		;NEW ANG← OLD ANG + 1/2 CHANGE IN ANG
UPHALF:	MOV	(R0)+,R3	;FUTURE JOINT ANGLE
	SUB	(R1),R3		;OLD
	ASR	R3
	ADD	R3,(R1)+	;NEW ANGLE
	SOB	R2,UPHALF
	BR	SETDAC	

CALRUN:	BIT	#WORKNG,@#ARMS	;RUNSUB STILL WORKING?
	BEQ	RUNOK 
	MOV	#RUNERR,R1	;SIGNAL RUNSUB CLOCK OVERRUN
	MOV	#CANPRO,R0
	BR	SIGERR

RUNOK: 	MOV	@#CBUF,R0	;NEW ANGLE ← FUTURE ANGLE
	TST	(R0)+
	MOV	(R0)+,(R1)+
	SOB	R2,.-2
	BIT	#FINAL,(R0)	;FINAL SET POINT?
	BNE	ISFINL		;BRANCH IF YES
	BIS	#STRWRK,@#ARMS	;EXECUTE RUNSUB IF NOT
	BR	SETDAC

ISFINL:	MOV	BRAKES(R4),R0	;ELSE RE-ENABLE HARDWARE INTEGRATION
	BIS	@#MODES,R0
	BIC	#NOINTG,R0
	JSR	PC,RELBRK
	BIS	#CHKDNE,@#ARMS	;INDICATE JUST NULLING ERRORS

SETDAC:	MOV	FSTJT(R4),R0	;STILL RUNNING, OUTPUT DAC VALUES
	MOV	NUMJT(R4),R1
	JSR	PC,DACOUT

	BIT	#STRWRK,@#ARMS	;START UP RUNSUB?
	BEQ	CLKDNE
	ADD	#STRWRK,@#ARMS	;INDICATE WORKING
	MOV	@#PTIME,R0	;TIME TO EVALUATE SET POINT
	ADD	#2,R0
	MOV	@#CBUF,R1	;POINT TO JOINT ANGLE BUFFER TO FILL
	TST	(R1)+
	MTPS	#UNLOCK		;ALLOW CLOCK INTERRUPTS
	JSR	PC,@RUNSUB(R4)
	BIC	#WORKNG,@#ARMS
	BIT	#STPARM,@#ARMS	;STOP ARM IMMEDIATELY?
	BNE	STPJT		;YES
	BR	CLKDNE		;NO

SIGERR:	MOVB	R1,@#ARMS	;SET ERROR BITS
	BIS	R0,@#ARMS
STPJT:	JSR	PC,SETBRK     	;SET JOINT BRAKES, DISABLE INTERFACE
	BIC	#177000,@#ARMS

CLKDNE:	MOV	(SP)+,R5
	MOV	(SP)+,R4
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	MOV	(SP)+,R1
	MOV	(SP)+,R0
	RTI

;LOCAL STORAGE AREA

PTIME:	0	;TIME INTO CURRENT JOINT MOTION ( IN 8 MSEC. COUNTS )
DRVLST:	0	;POINTER TO TABLE CONTAINING MOTION VARIABLE
DNECNT:	0	;COUNTER FOR NUMBER OF SEQUENTIAL TIMES IN RANGE TRUE
CBUF:	BUF1	;POINTER TO CURRENT JOINT ANGLE BUFFER
BUF1:	BUF2	;RING OF JOINT ANGLE BUFFERS
	.=.+20
BUF2:	BUF1
	.=.+20


;END OF "DRIVE" ROUTINE
;"ANGLES" - SUBR. TO READ CURRENT JOINT ANGLES AND HAND OPENING

;JOINT ANGLES ARE READ FROM THE A/D AND CONVERTED INTO DEGREES AND
;INCHES.  THE FIRST JOINT TO BE READ MUST BE SPECIFIED BY REGISTER
;R0 ( I.E. 1-6 FOR THE ARM JOINTS AND 7 FOR THE HAND ) AND THE NUMBER
;OF JOINTS TO BE READ MUST BE SPECIFIED IN R1. THE CONVERTED READINGS
;ARE LEFT IN THE ARRAY "JANGLES" IN THERE APPROPRIATE ELEMENTS ( I.E.
;JOINT #1 IN JANGLE, JOINT #2 IN JANGLE+2, ETC. ).  A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
;		MOV	#FRSTJT,R0	;# OF FIRST JOINT TO BE READ
;		MOV	#JTS,R1		;NUMBER OF JOINTS TO BE READ
;		JSR	PC,ANGLES	;NO ARGUMENTS REQUIRED
;
;IF THE ADC READ TAKES TOO LONG TO EXECUTE, THE C BIT IS SET, "ADERR"
;IS INDICATED IN THE ARM STATUS WORD, AND R1 CONTAINS AN ERROR CODE
;WHEN THIS ROUTINE EXITS.  OTHERWISE, THE C BIT IS CLEARED.

;REGISTERS USED:
;	R0 AND R1 PASS ARGUMENTS AND ARE GARBAGED

ANGLES:	MOV	R2,-(SP)
	MOV	R3,-(SP)
	DEC	R0		;CONVERT JOINT NUMBER TO WORD POINTER
	ASL	R0
	BIC	#3+ADCENB,@#DR11S	;SET DR11 TO ADC MODE
	BIS	#ADCMDE,@#DR11S	
       	MOV	ACHAN(R0),@#DR11O	;SEND OFF FIRST POT CHANNEL
ANGL1:	MOV	#100.,R3	;SET A/D CONVERSION TIME OUT COUNT
ANGL2:	BIT	#ADCDNE,@#DR11S	;CHECK IF  A/D CONVERSION COMPLETED
	BNE	ANGL3		;BRANCH IF COMPLETED
	SOB	R3,ANGL2	;LOOP TILL TIME RUNS OUT
	BIS	#STPARM,@#ARMS	;INDICATE ADC ERROR, CAN'T PROCEED
	MOV	#ADCERR,R1	;SIGNAL ERROR AND EXIT
	SEC
	BR	ANGDNE		
ANGL3:	MOV	@#DR11I,R2	;GET ADC READING
	MOV	ACHAN+2(R0),@#DR11O	;START CONVERTING NEXT POT
	MUL 	SCALE(R0),R2 	;CONVERT TO DEG. OR INCHES
	ASHC	#4,R2
	ADD 	OFFSET(R0),R2 	;APPLY POT OFFSET
	MOV	R2,JANGLE(R0)	;SAVE NEW ANGLE IN "JANGLE"
	TST	(R0)+
	SOB	R1,ANGL1	;REPEAT FOR ALL REQUESTED JOINTS
	CLC
ANGDNE:	MOV	(SP)+,R3
	MOV	(SP)+,R2
	RTS	PC

;END OF "ANGLES"
;"DACOUT" - SUBR. TO OUTPUT DAC VALUES GIVEN JOINT ANGLES       

;THIS ROUTINE REQUIRES TWO ARGUMENTS, THE LOGICAL NUMBER OF THE FIRST
;JOINT TO HAVE IT'S DAC OUTPUT SET AND THE NUMBER OF JOINTS TO BE
;SERVICED.  ALL JOINTS FROM THE 1ST. JOINT TO (1ST JOINT -1 + NUMBER
;OF JOINTS) HAVE THEIR DAC VALUES SET.  THE DAC VALUES ARE TAKEN FROM
;THE ARRAY "JANGLE" AND ARE CONVERTED TO DAC UNITS VIA THE CONSTANTS
;BOFFST AND BSCALE.
;
;		MOV	#FRSTJT,R0	;NUMBER OF 1ST JOINT 
;		MOV	#JTS,R1		;# OF JOINTS TO BE SERVICED
;		JSR	PC,DACOUT	;NO ARGUMENTS REQUIRED
;
;THIS ROUTINE NEVER RETURNS AN ERROR CODE.

;REGISTERS USED:
;	R0 AND R1 PASS ARGUMENTS AND ARE GARBAGED
;	R2,R3 ARE GARBAGED

DACOUT:	DEC	R0		;CONVERT JOINT NUMBER TO WORD POINTER
	ASL	R0
	BIC	#3,@#DR11S	;DR11 DAC MODE
DACO1:	MOV	JANGLE(R0),R2  	;DESIRED JOINT ANGLE
	SUB	BOFFST(R0),R2	;SUBT. POT OFFSET
	MUL	BSCALE(R0),R2	;CONVERT TO DAC UNITS
	ASHC	BSHIFT(R0),R2
	BIC	#170000,R2	;CLEAR ROOM FOR CHANNEL, POS. MODE
	BIS	DCHAN(R0),R2	;ADD DAC CHANNEL
	MOV	R2,@#DR11O   	;OUTPUT DAC READING AND CHANNEL
	TST	(R0)+
	SOB	R1,DACO1
	RTS	PC

;END OF "DACOUT"
;"RELBRK"&"SETBRK" - SUBR. TO RELEASE AND SET BRAKE BITS        

;THE BRAKE BITS TO BE MODIFIED MUST BE LOADED INTO REGISTER R0 BEFORE
;CALLING "RELBRK".  "SETBRK" DOES NOT REQUIRE AN ARGUMENT.  A SAMPLE
;SERIES OF CALLS TO THESE ROUTINES FOLLOWS:
;
;		MOV	#BRAKES,R0	;SEND BRAKE BITS
;		JSR	PC,RELBRK
;			or
;		JSR	PC,SETBRK
;
;THE ARM INTERFACE IS ENABLED AND DISABLED BY THESE ROUTINES AS 
;NECESSARY.  IF "RELBRK" IS UNABLE TO ENABLE THE HARDWARE SERVO,
;THE ERROR BIT "NOTENB" IS SET IN "ARMS" AND THE C BIT IS SET
;WHEN THIS ROUTINE EXITS.

;REGISTERS USED:
;
;	R0 PASSES ARGUMENTS AND IS  MODIFIED
;	NO OTHER REGISTERS AFFECTED

RELBRK:	MOV	#BRKMDE,@#DR11S	;SET BRAKE BIT MODE
	BIS	#ENABLE,R0   	;ENABLE INTERFACE TO COMPUTER MODE
	MOV	R0,@#DR11O	;RELEASE APPROPRIATE BRAKES
	MOV	#STTMDE,@#DR11S	;CHECK THE IF HARDWARE SERVO NOW ON
	MOV	@#DR11I,R0
	CLC			;ASSUME SERVO ENABLED
	BIT	#ISON,R0	;TEST IF ENABLED
	BNE	RELDNE		;EXIT IF EVERYTHING OK
	BIS	#CANPRO,@#ARMS	;ELSE INDICATE ERROR
	MOVB	#NOHDWR,@#ARMS
	SEC
RELDNE:	RTS	PC


SETBRK:	MOV	#BRKMDE,@#DR11S	;SET BRAKE BIT MODE
	CLR	@#DR11O		;DISABLE ARM INTERFACE, SET BRAKES
	RTS	PC
	

;END OF BRAKE ROUTINES
;ADC AND DAC CHANNELS FOR ARM JOINTS

ACHAN:	0			;ADC CHANNELS, JTS 1-7
	1
	2
	3
	4
	5
	6

DCHAN:	00000			;DAC CHANNELS, JTS 1-7
	20000
	40000
	60000
	100000
	120000
	140000

.END